%clear all;
%clc;

TEST = 4; % 1 -- Testing model
          % 2 -- Fluctuatic Errors
          % 3 -- Dinamic Errors
          % 4 -- Summary Errors

T = 0.05;
if TEST == 1
    Tmax = 5;
else
    Tmax = 3600;
end
t = T:T:Tmax;
N = length(t);

F = [1 T;
     0 1];
G = [0 0;
     0 T];

if TEST == 1
    Dxi = 0;
    Deta = 0;
elseif TEST == 2
    Dxi = 0;
    Deta = 7;
elseif TEST == 3
    Dxi = 12;
    Deta = 0;
else
    Dxi = 12;
    Deta = 7;
end

band_plot = 2;



Omega = nan(1,N);
InOmega = nan(1,N);
ErrOmega = nan(1,N);


if TEST == 1
    band = band_plot;
else
    band = 0.1:0.1:3;
end

RMS_Omega = nan(1,length(band));
for i = 1:length(band)
    if TEST == 1
        Xist = [10;0];
    else
        Xist =[0;0];
    end
    Xest = [0;0];
    Xext = [0;0]; %F*Xest;
    K = [8/3*band(i)*T;
         32/9*band(i)^2*T];
    xi = sqrt(Dxi) * randn(1,N);
    eta = sqrt(Deta) * randn(1,N);

    for k = 1:N
        Xist = F * Xist + G *[0; xi(k)];
        InOmega(k) = Xist(1);
        omega_meas = Xist(1) + eta(k);
        Xest = Xext + K * (omega_meas - Xext(1));
        Xext = F * Xest;
        Omega(k) = Xext(1);
        ErrOmega(k) = Xest(1) - Xist(1);
    end
    RMS_Omega(i) = sqrt(mean(ErrOmega .^ 2));
    
    if TEST == 1
        figure(1);
        plot(t,[Omega;ErrOmega;InOmega]);
        grid on;
        title(['Bandwidth = ' num2str(band(i)) 'Hz']);
        xlabel('t, sec');
        ylabel('\omega, rad');
        saveas(1, 'test_model.png');
    end
end

Col = [0,0,0];

if and(Dxi == 0,not(Deta == 0))
    Col = [0,0,1];
    leg{legi} = 'Fluctuatic';
    legi = legi + 1;
elseif and(Deta == 0,not(Dxi == 0))
    Col = [0,1,0];
    leg{legi} = 'Dinamic';
    legi = legi + 1;
elseif and(not(Dxi == 0),not(Deta == 0))
    Col = [1,0,0];
    SAVE = 1;
    leg{legi} = 'Summary';
    legi = legi + 1;
end

if not(TEST == 1)
    figure(2);
    hold on;
    plot(band,RMS_Omega,'Color',Col);
    legend(leg);
    xlabel('Bandwidth, Hz');
    ylabel('RMS \omega, Hz');
    grid on;
    hold off
    if SAVE == 1
        saveas(2,'Errors.png')
    end
end

if TEST == 1
    clear all;
    clc;
    legi = 1;
    leg = {'','',''};
    SAVE = 0;
end